public Model3DGroup Create(Color modelColor,string pictureName, Point3D startPos, double maxHigh)
{
try
{
Uri inpuri = new Uri(@pictureName, UriKind.Relative);
BitmapImage bi = new BitmapImage();
bi.BeginInit();
bi.UriSource = inpuri;
bi.EndInit();
ImageBrush imagebrush = new ImageBrush(bi);
imagebrush.Opacity = 100;
imagebrush.Freeze();
Point[] ptexture0 = { new Point(0, 0), new Point(0, 1), new Point(1, 0) };
Point[] ptexture1 = { new Point(1, 0), new Point(0, 1), new Point(1, 1) };
SolidColorBrush modelbrush = new SolidColorBrush(modelColor);
Model3DGroup cube = new Model3DGroup();
Point3D uppercircle = startPos;
modelbrush.Freeze();
uppercircle.Y = startPos.Y + maxHigh;
cube.Children.Add(CreateEllipse2D(modelbrush, uppercircle, _EllipseHigh, new Vector3D(0, 1, 0)));
cube.Children.Add(CreateEllipse2D(modelbrush, startPos, _EllipseHigh, new Vector3D(0, -1, 0)));
cube.Children.Add(CreateEllipse3D(imagebrush, startPos, _EllipseHigh, maxHigh, ptexture0));
return cube;
}
catch (Exception ex)
{
throw ex;
}
}
/// <summary>
///
/// </summary>
/// <param name="origin"></param>
/// <param name="dirX"></param>
/// <returns></returns>
public static Matrix44 GetLCSMatrix(WM.Point3D origin, WM.Vector3D dirX)
{
dirX.Normalize();
Vector3D localX = new Vector3D(dirX.X, dirX.Y, dirX.Z);
Vector3D localZ = new Vector3D(0, 0, 1);
if (GeomOperation.IsCollinear(localZ, localX, MathConstants.ZeroWeak)) //MathConstants.ZeroGeneral))
{
// When X is vertical then Z goes in X dir or -X
//localZ = new Vector3D(0, 1, 0);
if (localX.DirectionZ > 0)
{
localZ = new Vector3D(-1, 0, 0);
}
else
{
localZ = new Vector3D(1, 0, 0);
}
}
//Vector3D localY = (localZ * localX).Normalize;
//Matrix44 matrix = new Matrix44(origin, localX, localY);
Vector3D localY = (localZ * localX);
localZ = (localX * localY).Normalize;
Matrix44 matrix = new Matrix44(origin, localX.Normalize, localY.Normalize, localZ);
return(matrix);
}
public object Convert(object value, Type targetType, object parameter, System.Globalization.CultureInfo culture)
{
var trafo = ((Transform3D)value).Value;
var pos = new System.Windows.Media.Media3D.Point3D(trafo.OffsetX, trafo.OffsetY, trafo.OffsetZ);
return(pos);
}
public void getData()
{
context.WaitOneUpdateAll(depth);
depth.GetMetaData(depthMD);
System.Windows.Media.Media3D.Point3D u_new, u_real, f, c;
c = new System.Windows.Media.Media3D.Point3D(0, 0, centerpointDepth);
OpenNI.Point3D newF;
newF = Change(depthMD, backgroundDepthMD); //returns the pixal pos
if (newF.X != 0 && newF.Z != 0 && newF.Z != 0)
{
newF = depth.ConvertProjectiveToRealWorld(newF);
f = new System.Windows.Media.Media3D.Point3D(newF.X, newF.Y, newF.Z); // the point of intress
u_new = Translation(f, c); //A translation that brings point 1 to the origin
float d = Distance(scene.Floor, newF);
double angleInRadians = (Math.PI / 180) * angleBetween;;
double angleInRadiansY = (Math.PI / 180) * angleBetweenY;;
var matrix = NewRotateAroundX(angleInRadians); // Rotation around the origin by the required angle
matrix = NewRotateAroundY(angleInRadiansY);
u_real = Multiplication(matrix, u_new);
//Console.WriteLine("new U {0}, {1}, {2} ", (int)u_new.X, (int)u_new.Y, (int)u_new.Z);
//Console.WriteLine("u_real , {1}, {2} and {3} distance {4}", depthMD.FrameID, (int)u_real.X, (int)u_real.Y, (int)u_real.Z, d);
xYDepth = new System.Windows.Media.Media3D.Point3D(u_real.X, u_real.Y, d);
}
}
public static Vector3 Add(this Vector3 vector3, System.Windows.Media.Media3D.Point3D point3D)
{
vector3.X += (float)point3D.X;
vector3.Y += (float)point3D.Y;
vector3.Z += (float)point3D.Z;
return(vector3);
}
/// <summary>
/// Gets relative position of <paramref name="testedPt"/> relatively to the oriented line <paramref name="start"/> - <paramref name="end"/>
/// </summary>
/// <param name="start">Start point of the oriented line</param>
/// <param name="end">End point of the oriented line</param>
/// <param name="testedPt">Tested point</param>
/// <returns>Relative position of <paramref name="testedPt"/></returns>
public static double GetRelativePos(ref WM.Point3D start, ref WM.Point3D end, ref WM.Point3D testedPt)
{
var origVect = end - start;
double origVectLen = origVect.Length;
var posVect = testedPt - start;
double posVectLen = origVect.Length;
if (origVectLen.IsZero())
{
if (posVectLen.IsZero())
{
return(0);
}
else
{
return(double.NaN);
}
}
var weldDir = origVect;
weldDir.Normalize();
double vectDot = WM.Vector3D.DotProduct(posVect, weldDir);
if (vectDot.IsEqual(origVectLen))
{
return(1);
}
double relPos = (vectDot / origVectLen);
return(relPos);
}
public static MeshGeometry3D AddPlaneToMesh(MeshGeometry3D mesh, Vector3D normal, Point3D upperLeft, Point3D lowerLeft, Point3D lowerRight, Point3D upperRight)
{
int offset = mesh.Positions.Count;
mesh.Positions.Add(upperLeft);
mesh.Positions.Add(lowerLeft);
mesh.Positions.Add(lowerRight);
mesh.Positions.Add(upperRight);
mesh.Normals.Add(normal);
mesh.Normals.Add(normal);
mesh.Normals.Add(normal);
mesh.Normals.Add(normal);
mesh.TextureCoordinates.Add(new Point(0, 0));
mesh.TextureCoordinates.Add(new Point(0, 1));
mesh.TextureCoordinates.Add(new Point(1, 1));
mesh.TextureCoordinates.Add(new Point(1, 0));
mesh.TriangleIndices.Add(offset + 0);
mesh.TriangleIndices.Add(offset + 1);
mesh.TriangleIndices.Add(offset + 2);
mesh.TriangleIndices.Add(offset + 0);
mesh.TriangleIndices.Add(offset + 2);
mesh.TriangleIndices.Add(offset + 3);
return mesh;
}
public void RotateObject(double horizontalRotation, double verticalRotation, Point3D objectCenter)
{
const double PositionChangeScale = 100.0; // Determined from user experience
var lookDirection = ToPantoVector3D(Camera.LookDirection);
//Point3D rotationPoint;
//if (Math.Abs(lookDirection.Z) < 1e-6)
//{
// rotationPoint = new Point3D(0, 0, 0);
//}
//else
//{
// // Calculate scaling of look direction vector to hit the object plane.
// // Object plane is here defined as x-y-plane with z = 0.
// var intersectionVectorScaling = -(Camera.Position.Z - objectCenter.Z) / lookDirection.Z;
// rotationPoint = new Point3D(
// Camera.Position.X + intersectionVectorScaling * lookDirection.X,
// Camera.Position.Y + intersectionVectorScaling * lookDirection.Y,
// Camera.Position.Z + intersectionVectorScaling * lookDirection.Z);
//}
var lookDirectionMagnitude = lookDirection.Magnitude();
var rotationPointScaling = 1000 / lookDirectionMagnitude;
var rotationPoint = new Point3D(
Camera.Position.X + rotationPointScaling * lookDirection.X,
Camera.Position.Y + rotationPointScaling * lookDirection.Y,
Camera.Position.Z + rotationPointScaling * lookDirection.Z);
var distanceToRotationPoint = ToPantoVector3D(rotationPoint - Camera.Position).Magnitude();
// Move camera up and sideways in current image plane
var positionChange = CalculatePositionChange(horizontalRotation, verticalRotation)
.Multiply(PositionChangeScale)
.ToVector3D();
var pannedPosition = new Point3D(
Camera.Position.X + positionChange.X,
Camera.Position.Y + positionChange.Y,
Camera.Position.Z + positionChange.Z);
// Point camera at rotation point
var newLookDirection = new Commons.Mathematics.Vector3D(
rotationPoint.X - pannedPosition.X,
rotationPoint.Y - pannedPosition.Y,
rotationPoint.Z - pannedPosition.Z)
.Normalize()
.ToVector3D();
// Do not transform if new look direction almost straight up/down
if (Math.Abs(newLookDirection.Y) / newLookDirection.Magnitude() > 0.9) // Found from user experience
{
return;
}
Camera.LookDirection = new Vector3D(newLookDirection.X, newLookDirection.Y, newLookDirection.Z);
// Ensure that distance to rotation point is preserved by moving camera towards it
var newDistanceToRotationPoint = ToPantoVector3D(rotationPoint - pannedPosition).Magnitude();
var distancePreservedPosition = new Point3D(
pannedPosition.X + (newDistanceToRotationPoint - distanceToRotationPoint) * newLookDirection.X,
pannedPosition.Y + (newDistanceToRotationPoint - distanceToRotationPoint) * newLookDirection.Y,
pannedPosition.Z + (newDistanceToRotationPoint - distanceToRotationPoint) * newLookDirection.Z);
Camera.Position = distancePreservedPosition;
}
public static void drawTor(Point3D center, double R, double r, int N, int n, Color color, Viewport3D mainViewport)
{
if (n < 2 || N < 2)
{
return;
}
Model3DGroup tor = new Model3DGroup();
Point3D[,] points = new Point3D[N, n];
for (int i = 0; i < N; i++)
{
for (int j = 0; j < n; j++)
{
points[i, j] = getPositionTor(R, r, i * 360 / (N - 1), j * 360 / (n - 1));
points[i, j] += (Vector3D)center;
}
}
Point3D[] p = new Point3D[4];
for (int i = 0; i < N - 1; i++)
{
for (int j = 0; j < n - 1; j++)
{
p[0] = points[i, j];
p[1] = points[i + 1, j];
p[2] = points[i + 1, j + 1];
p[3] = points[i, j + 1];
drawTriangle(p[0], p[1], p[2], color, mainViewport);
drawTriangle(p[2], p[3], p[0], color, mainViewport);
}
}
}
public Star()
{
starColor = foregroundBrush;
starGFX = new Ellipse();
starSEL = new Ellipse();
starCanvas = new Canvas();
starLabel = new Label();
starSEL.Visibility = Visibility.Hidden;
starGFX.Fill = foregroundBrush;
starLabel.FontFamily = new FontFamily(new Uri("pack://application:,,,/Fonts/"), "./#Euro Caps");
starLabel.Foreground = fontBrush;
starCanvas.Children.Add(starLabel);
starCanvas.Children.Add(starGFX);
starCanvas.Children.Add(starSEL);
movePoint = new Point3D(0,0,0);
rotaPoint = new Point3D(0,0,0);
starCanvas.MouseLeftButtonDown += SetSelection;
starCanvas.MouseRightButtonDown +=TargetSelection;
starCanvas.MouseEnter += ShowInfoSelection;
starCanvas.MouseLeave += HideInfoSelection;
}
private static ModelVisual3D Planar(IFeature basePath)
{
TongJi.Geometry.Polyline curve = new TongJi.Geometry.Polyline(basePath.GeoData);
if (curve.AlgebraicArea < 0)
{
curve = curve.ReversePoints();
}
var points = curve.Points.ToList();
if (points.Last() == points.First())
{
points.RemoveAt(points.Count - 1);
}
var triangles = DelaunayOfPolygon(points);
MeshBuilder mb = new MeshBuilder();
foreach (var tri in triangles)
{
var a = new Point3D(tri.A.x, tri.A.y, 0);
var b = new Point3D(tri.B.x, tri.B.y, 0);
var c = new Point3D(tri.C.x, tri.C.y, 0);
mb.AddTriangle(a, b, c);
}
ModelVisual3D visual = new ModelVisual3D();
visual.Content = new GeometryModel3D(mb.ToMesh(), MaterialHelper.CreateMaterial(Colors.Blue));
return(visual);
}
public YPitchBendCube(Point3D center, double radius,
Pitch pitch, Instrument instrument, OutputDevice device, Channel channel)
: base(center, radius, new InstrumentNoteAction(device, channel, pitch)) {
outputDevice = device;
this.channel = channel;
}
internal static void SetPoint(this User user, JointID joint, Point3D newPoint)
{
switch (joint)
{
case JointID.Head:
user.Head = newPoint;
break;
case JointID.AnkleLeft:
user.AnkleLeft = newPoint;
break;
case JointID.ElbowLeft:
user.ElbowLeft = newPoint;
break;
case JointID.FootLeft:
user.FootLeft = newPoint;
break;
case JointID.HandLeft:
user.HandLeft = newPoint;
break;
case JointID.KneeLeft:
user.KneeLeft = newPoint;
break;
case JointID.ShoulderLeft:
user.ShoulderLeft = newPoint;
break;
case JointID.HipLeft:
user.HipLeft = newPoint;
break;
case JointID.ShoulderCenter:
user.ShoulderCenter = newPoint;
break;
case JointID.AnkleRight:
user.AnkleRight = newPoint;
break;
case JointID.ElbowRight:
user.ElbowRight = newPoint;
break;
case JointID.FootRight:
user.FootRight = newPoint;
break;
case JointID.HandRight:
user.HandRight = newPoint;
break;
case JointID.KneeRight:
user.KneeRight = newPoint;
break;
case JointID.HipRight:
user.HipRight = newPoint;
break;
case JointID.ShoulderRight:
user.ShoulderRight = newPoint;
break;
case JointID.Spine:
user.Spine = newPoint;
break;
case JointID.HipCenter:
user.HipCenter = newPoint;
break;
}
}
protected override Geometry3D CreateMesh()
{
_radius = Radius;
_position = Position;
MeshGeometry3D mesh = new MeshGeometry3D();
Point3D prevPoint = PointForAngle(0);
Vector3D normal = new Vector3D(0, 0, 1);
const int div = 180;
for (int i = 1; i <= div; ++i)
{
double angle = 2 * Math.PI / div * i;
Point3D newPoint = PointForAngle(angle);
mesh.Positions.Add(prevPoint);
mesh.Positions.Add(_position);
mesh.Positions.Add(newPoint);
mesh.Normals.Add(normal);
mesh.Normals.Add(normal);
mesh.Normals.Add(normal);
prevPoint = newPoint;
}
mesh.Freeze();
return mesh;
}
/// <summary>
/// Generates a flat mesh starting at origin with sides equal to u and v vecotrs
/// </summary>
/// <param name="origin"></param>
/// <param name="u"></param>
/// <param name="v"></param>
/// <param name="usteps"></param>
/// <param name="vsteps"></param>
/// <param name="textureBounds"></param>
/// <returns></returns>
public static MeshGeometry3D CreateMesh(Point3D origin, Vector3D u, Vector3D v, int usteps, int vsteps, Rect textureBounds)
{
u = 1.0 / usteps * u;
v = 1.0 / vsteps * v;
MeshGeometry3D mesh = new MeshGeometry3D();
for (int i = 0; i <= usteps; i++)
{
for (int j = 0; j <= vsteps; j++)
{
mesh.Positions.Add(origin + i * u + j * v);
mesh.TextureCoordinates.Add(new Point(textureBounds.X + textureBounds.Width * i / usteps,
textureBounds.Y + textureBounds.Height * j / vsteps));
if (i > 0 && j > 0)
{
mesh.TriangleIndices.Add((i - 1) * (vsteps + 1) + (j - 1));
mesh.TriangleIndices.Add((i - 0) * (vsteps + 1) + (j - 0));
mesh.TriangleIndices.Add((i - 0) * (vsteps + 1) + (j - 1));
mesh.TriangleIndices.Add((i - 1) * (vsteps + 1) + (j - 1));
mesh.TriangleIndices.Add((i - 1) * (vsteps + 1) + (j - 0));
mesh.TriangleIndices.Add((i - 0) * (vsteps + 1) + (j - 0));
}
}
}
return mesh;
}
/// <summary>
/// Gets intersection of two planes
/// </summary>
/// <param name="plane1">The first plane</param>
/// <param name="plane2">The second plane</param>
/// <param name="pointOnLine">Point on line where <paramref name="plane1"/> and <paramref name="plane1"/> are intersecting</param>
/// <param name="dirVect">Direction vector of the intersection</param>
/// <returns>True if intersection exists</returns>
public static bool GetIntersection(Plane3D plane1, Plane3D plane2, out WM.Point3D pointOnLine, out WM.Vector3D dirVect)
{
#if DEBUG
// Normal vectors should be normalized
WM.Vector3D testVect1 = plane1.NormalVector;
testVect1.Normalize();
WM.Vector3D testVect2 = plane2.NormalVector;
testVect2.Normalize();
Debug.Assert(WM.Vector3D.DotProduct(plane1.NormalVector, plane2.NormalVector).IsEqual(WM.Vector3D.DotProduct(testVect1, testVect2)));
#endif
double normalDot = WM.Vector3D.DotProduct(plane1.NormalVector, plane2.NormalVector);
double D = (1 - normalDot * normalDot);
if (D.IsZero())
{
pointOnLine = new WM.Point3D(double.NaN, double.NaN, double.NaN);
dirVect = new WM.Vector3D(double.NaN, double.NaN, double.NaN);
return(false);
}
double h1 = -1 * plane1.GetD();
double h2 = -1 * plane2.GetD();
double c1 = (h1 - h2 * normalDot) / D;
double c2 = (h2 - h1 * normalDot) / D;
dirVect = WM.Vector3D.CrossProduct(plane1.NormalVector, plane2.NormalVector);
dirVect.Normalize();
pointOnLine = (WM.Point3D)(c1 * plane1.NormalVector + c2 * plane2.NormalVector);
return(true);
}
/// <summary>
/// Gets distance of the <paramref name="point"/> from <paramref name="line"/>
/// </summary>
/// <param name="line">Line</param>
/// <param name="point">Point</param>
/// <returns>Distance</returns>
public static double Distance(ref StraightLine line, ref WM.Point3D point)
{
Plane3D tempPln = new Plane3D(point, line.Direction);
var temPlnIntersection = tempPln.GetIntersection(ref line);
return((temPlnIntersection - point).Length);
}
public void Reverse()
{
var p = P1;
P1 = P2;
P2 = p;
}
public Viewer3D()
{
InitializeComponent();
DataContextChanged += Viewer3D_DataContextChanged;
//if(Tag.ToString() == "Z")
//{
// return;
//}
// Setta to XY
Vector3D faceNormal = new Vector3D(0, 0, 1);
Vector3D faceUp = new Vector3D(0, 1, 0);
var camera = view.Viewport.Camera as ProjectionCamera;
Point3D target = camera.Position + camera.LookDirection;
double dist = camera.LookDirection.Length;
Vector3D lookdir = -faceNormal;
lookdir.Normalize();
lookdir = lookdir * dist;
Point3D pos = target - lookdir;
Vector3D updir = faceUp;
updir.Normalize();
CameraHelper.AnimateTo(camera, pos, lookdir, updir, 500);
//view.Orthographic = true;
//view.Camera.NearPlaneDistance = 0;
//view.Camera.FarPlaneDistance = 10000;
}
//Find the line of intersection between two planes.
//The inputs are two game objects which represent the planes.
//The outputs are a point on the line and a vector which indicates it's direction.
internal static bool PlanesIntersection(Plane3D plane1, Plane3D plane2, out StraightLine intersection)
{
var linePoint = new WM.Point3D();
var lineVec = new WM.Vector3D();
//Get the normals of the planes.
var plane1Normal = plane1.NormalVector;
var plane2Normal = plane2.NormalVector;
//We can get the direction of the line of intersection of the two planes by calculating the
//cross product of the normals of the two planes. Note that this is just a direction and the line
//is not fixed in space yet.
lineVec = WM.Vector3D.CrossProduct(plane1Normal, plane2Normal);
//Next is to calculate a point on the line to fix it's position. This is done by finding a vector from
//the plane2 location, moving parallel to it's plane, and intersecting plane1. To prevent rounding
//errors, this vector also has to be perpendicular to lineDirection. To get this vector, calculate
//the cross product of the normal of plane2 and the lineDirection.
var ldir = WM.Vector3D.CrossProduct(plane2Normal, lineVec);
var numerator = WM.Vector3D.DotProduct(plane1Normal, ldir);
//Prevent divide by zero.
if (Math.Abs(numerator) > 0.000001)
{
var plane1ToPlane2 = plane1.PointOnPlane - plane2.PointOnPlane;
var t = WM.Vector3D.DotProduct(plane1Normal, plane1ToPlane2) / numerator;
linePoint = plane2.PointOnPlane + t * ldir;
intersection = new StraightLine(ref linePoint, ref lineVec);
return(true);
}
intersection = default(StraightLine);
return(false);
}
private void imageshow_MouseLeftButtonDown(object sender, MouseButtonEventArgs e)
{
CoordinateTransformation dd = new CoordinateTransformation();
Mat rgb = null;
string rgbpath = System.IO.Path.Combine(Global.ImageDataPath, "rgb_" + pCurrentImage.ToString("D5") + ".jpg");
rgb = Cv2.ImRead(rgbpath);
OpenCvSharp.Size imagesize = rgb.Size();
int inputx = ((int)e.GetPosition(imageshow).X *imagesize.Width) / ((int)imageshow.ActualWidth);
int inputy = ((int)e.GetPosition(imageshow).Y *imagesize.Height) / ((int)imageshow.ActualHeight);
pt = dd.getvalue(pCurrentImage, inputx, inputy);
// Console.WriteLine(pt);
if (pt != new Point3D(0, 0, 0))
{
VIewer3D.Instance.ClickedCoordinate();
}
}
private Point3D CalculateCenter()
{
var center = new Point3D();
long counter = 0;
foreach (var model in _modelGroup.Children)
{
if (model is GeometryModel3D)
{
var geo = ((model as GeometryModel3D).Geometry as MeshGeometry3D);
if (geo != null)
{
foreach (var p in geo.Positions)
{
center.X += p.X;
center.Y += p.Y;
center.Z += p.Z;
counter++;
}
}
}
}
if (counter > 0)
{
center.X /= counter;
center.Y /= counter;
center.Z /= counter;
}
return(center);
}
public Star()
{
centerX = 285;
centerY = 285;
starGFX = new Ellipse();
starSEL = new Ellipse();
starCanvas = new Canvas();
starLabel = new Label();
starSEL.Visibility = Visibility.Hidden;
starGFX.Fill = backgroundBrush;
starLabel.FontFamily = new FontFamily(new Uri("pack://application:,,,/Fonts/"), "./#Euro Caps");
starLabel.Foreground = backgroundBrush;
starCanvas.Children.Add(starGFX);
starCanvas.Children.Add(starLabel);
starCanvas.Children.Add(starSEL);
movePoint = new Point3D(0,0,0);
rotaPoint = new Point3D(0,0,0);
starCanvas.PreviewMouseDown += MoveToSystem;
}
public static bool TriangleRayIntersect(Point3D p0, Point3D p1, Point3D p2, Point3D rayOrig, Vector3D rayDir, out Point3D hitPos, out Vector3D surfaceNormal, out double distance)
{
var edge01 = p1 - p0;
var edge02 = p2 - p0;
surfaceNormal = Vector3D.CrossProduct(edge01, edge02);
surfaceNormal.Normalize();
hitPos = new Point3D();
distance = double.MaxValue;
var p = Vector3D.CrossProduct(rayDir, edge02);
var det = Vector3D.DotProduct(edge01, p);
if (det < float.Epsilon) {
// レイとトライアングルが平行 or backface
return false;
}
var tvec = rayOrig - p0;
var u = Vector3D.DotProduct(tvec,p);
if (u < 0 || det < u) {
return false;
}
var qvec = Vector3D.CrossProduct(tvec, edge01);
var v = Vector3D.DotProduct(rayDir, qvec);
if (v < 0 || det < u+v) {
return false;
}
distance = Vector3D.DotProduct(edge02, qvec) / det;
hitPos = rayOrig + distance * rayDir;
return true;
}
/// <summary>
/// LookAtRH
/// </summary>
/// http://msdn.microsoft.com/en-us/library/bb281711(v=vs.85).aspx
/// <returns></returns>
public static Matrix3D SetViewMatrix(Point3D cameraPosition, Vector3D lookDirection, Vector3D upDirection)
{
// Normalize vectors:
lookDirection.Normalize();
upDirection.Normalize();
double dotProduct = Vector3D.DotProduct(lookDirection, upDirection);
// Define vectors, XScale, YScale, and ZScale:
double denom = Math.Sqrt(1 - Math.Pow(dotProduct, 2));
Vector3D XScale = Vector3D.CrossProduct(lookDirection, upDirection) / denom;
Vector3D YScale = (upDirection - dotProduct * lookDirection) / denom;
Vector3D ZScale = lookDirection;
// Construct M matrix:
Matrix3D M = new Matrix3D()
{
M11 = XScale.X, M12 = YScale.X, M13 = ZScale.X,
M21 = XScale.Y, M22 = YScale.Y, M23 = ZScale.Y,
M31 = XScale.Z, M32 = YScale.Z, M33 = ZScale.Z
};
// Translate the camera position to the origin:
Matrix3D translateMatrix = new Matrix3D();
translateMatrix.Translate(new Vector3D(-cameraPosition.X, -cameraPosition.Y, -cameraPosition.Z));
// Define reflect matrix about the Z axis:
Matrix3D reflectMatrix = new Matrix3D();
reflectMatrix.M33 = -1;
// Construct the View matrix:
Matrix3D viewMatrix = translateMatrix * M * reflectMatrix;
return viewMatrix;
}
public List<StarSystem> BuildStarSystemCollection(Point3D midPoint, double range)
{
List<StarSystem> returnList = new List<StarSystem>();
double minX = midPoint.X - range;
double minY = midPoint.Y - range;
double minZ = midPoint.Z - range;
double maxX = midPoint.X + range;
double maxY = midPoint.Y + range;
double maxZ = midPoint.Z + range;
foreach (var systemlist in milkyWay
.Where(system => system.Location.X >= minX && system.Location.X <= maxX &&
system.Location.Y >= minY && system.Location.Y <= maxY &&
system.Location.Z >= minZ && system.Location.Z <= maxZ))
{
if (calculateDistance(midPoint, new Point3D(systemlist.Location.X, systemlist.Location.Y, systemlist.Location.Z)) <= range)
{
returnList.Add(systemlist);
}
}
centerPoint = midPoint;
return returnList;
}
public static void DebugTracking(Point3D position, int leftSpeed, int rightSpeed, string message)
{
#if DEBUG_TRACKING
Console.WriteLine(message + " %(" + leftSpeed + ", " + rightSpeed + ") @(" +
Math.Round(position.X, 2) + ", " + Math.Round(position.Y, 2) + ", " + Math.Round(position.Z, 2) + ")\n");
#endif
}
/// <summary>
/// iterates through the list and averages factor many points into one super point
/// </summary>
/// <param name="input">List(Point3D)</param>
/// <param name="factor">int</param>
/// <returns>List(Point3D)</returns>
public static List<Point3D> averageSample(List<Point3D> input, int factor)
{
List<Point3D> output = new List<Point3D>();
for (int i = 0; i < input.Count; i++)
{
List<Point3D> sample = new List<Point3D>();
while ((i < input.Count) && (i % factor != factor - 1))
{
sample.Add(input[i]);
i++;
}
double aX = 0;
double aY = 0;
double aZ = 0;
for (int j = 0; j < sample.Count; j++)
{
aX = aX + sample[j].X;
aY = aY + sample[j].Y;
aZ = aZ + sample[j].Z;
}
aX = aX / sample.Count;
aY = aY / sample.Count;
aZ = aZ / sample.Count;
Point3D average = new Point3D(aX,aY,aZ);
output.Add(average);
}
return output;
}
public ExplosionWithVisual(Point3D centerPoint, double waveSpeed, double forceAtCenter, double maxRadius, Viewport3D viewport, double visualStartRadius)
: base(centerPoint, waveSpeed, forceAtCenter, maxRadius)
{
_viewport = viewport;
_visualStartRadius = visualStartRadius;
CreateVisual();
}
/// <summary>
/// Transform a point
/// </summary>
/// <param name="inPoint">Input point</param>
/// <param name="result">Output point</param>
/// <returns>True if the point was transformed successfuly, false otherwise</returns>
public bool TryTransform(Point inPoint, out Point3D result)
{
Point final2DPoint;
// assign this now so that we can return false if needed
result = new Point3D();
if (!_transform2D.TryTransform(inPoint, out final2DPoint))
{
return false;
}
Point texCoord = Viewport2DVisual3D.VisualCoordsToTextureCoords(final2DPoint, _childBounds);
// need to walk the texture coordinates on the Viewport2DVisual3D
// and look for where this point intersects one of them
Point3D coordPoint;
if (!Viewport2DVisual3D.Get3DPointFor2DCoordinate(texCoord,
out coordPoint,
_positions,
_textureCoords,
_triIndices))
{
return false;
}
if (!_transform3D.TryTransform(coordPoint, out result))
{
return false;
}
return true;
}
// Transfrom that moves the world to a camera coordinate system
// where the camera is at the origin looking down the negative z
// axis and y is up.
//
// NOTE: We consider camera.Transform to be part of the view matrix.
//
internal static Matrix3D CreateViewMatrix(Transform3D transform, ref Point3D position, ref Vector3D lookDirection, ref Vector3D upDirection)
{
Vector3D zaxis = -lookDirection;
zaxis.Normalize();
Vector3D xaxis = Vector3D.CrossProduct(upDirection, zaxis);
xaxis.Normalize();
Vector3D yaxis = Vector3D.CrossProduct(zaxis, xaxis);
Vector3D positionVec = (Vector3D) position;
double cx = -Vector3D.DotProduct(xaxis, positionVec);
double cy = -Vector3D.DotProduct(yaxis, positionVec);
double cz = -Vector3D.DotProduct(zaxis, positionVec);
Matrix3D viewMatrix = new Matrix3D(
xaxis.X, yaxis.X, zaxis.X, 0,
xaxis.Y, yaxis.Y, zaxis.Y, 0,
xaxis.Z, yaxis.Z, zaxis.Z, 0,
cx, cy, cz, 1);
PrependInverseTransform(transform, ref viewMatrix);
return viewMatrix;
}
/// <summary>
/// Get a single point along the curve
/// </summary>
/// <returns>
/// Got this here:
/// http://www.cubic.org/docs/bezier.htm
/// </returns>
public static Point3D GetPoint(double percent, Point3D[] controlPoints)
{
#region asserts
#if DEBUG
if (controlPoints.Length < 2)
{
throw new ArgumentException("There must be at least two points passed in: " + controlPoints.Length.ToString());
}
#endif
#endregion
Point3D[] prev = controlPoints;
Point3D[] current = null;
for (int outer = controlPoints.Length - 1; outer > 0; outer--)
{
current = new Point3D[outer];
for (int inner = 0; inner < outer; inner++)
{
current[inner] = Math3D.LERP(prev[inner], prev[inner + 1], percent);
}
prev = current;
}
return current[0]; // by the time execution gets here, the array only has one element
}
public FlipTile(DiffuseMaterial frontMaterial,
Size size, Point center, Material backMaterial, Rect backTextureCoordinates)
{
m_locationDesired = new Point3D(center.X, center.Y, 0);
m_locationCurrent = new Point3D(0, 0, Util.Rnd.NextDouble() * 10 - 20);
m_size = size;
Point3D topLeft = new Point3D(-size.Width / 2, size.Height / 2, 0);
Point3D topRight = new Point3D(size.Width / 2, size.Height / 2, 0);
Point3D bottomLeft = new Point3D(-size.Width / 2, -size.Height / 2, 0);
Point3D bottomRight = new Point3D(size.Width / 2, -size.Height / 2, 0);
m_frontMaterial = frontMaterial;
Model3DGroup quad = new Model3DGroup();
quad.Children.Add(
CreateTile(
frontMaterial,
backMaterial,
m_borderMaterial,
new Size3D(size.Width, size.Height, .01),
backTextureCoordinates));
Transform3DGroup group = new Transform3DGroup();
group.Children.Add(new RotateTransform3D(m_verticalFlipRotation));
group.Children.Add(new RotateTransform3D(m_quaternionRotation3D));
group.Children.Add(m_scaleTransform);
group.Children.Add(m_translate);
quad.Transform = group;
this.Visual3DModel = quad;
}
public BattleField3D()
{
InitializeComponent();
lookDirection = camera.LookDirection;
cameraPosition = camera.Position;
this.Loaded += (sender, e) => { this.Focus(); };
}
public static Point3D[] GetPoints(int count, Point3D[] controlPoints)
{
#region asserts
#if DEBUG
if (controlPoints.Length < 2)
{
throw new ArgumentException("There must be at least two points passed in: " + controlPoints.Length.ToString());
}
#endif
#endregion
double countD = count - 1;
Point3D[] retVal = new Point3D[count];
retVal[0] = controlPoints[0];
retVal[count - 1] = controlPoints[controlPoints.Length - 1];
for (int cntr = 1; cntr < count - 1; cntr++)
{
retVal[cntr] = GetPoint(cntr / countD, controlPoints);
}
return retVal;
}
/// <summary> Adds a point combined. </summary>
///
/// <param name="point"> The point. </param>
/// <param name="mesh"> The mesh. </param>
/// <param name="normal"> The normal. </param>
public static void addPointCombined(Point3D point, MeshGeometry3D mesh, Vector3D normal)
{
bool found = false;
int i = 0;
foreach (Point3D p in mesh.Positions)
{
if (p.Equals(point))
{
found = true;
mesh.TriangleIndices.Add(i);
mesh.Positions.Add(point);
mesh.Normals.Add(normal);
break;
}
i++;
}
if (!found)
{
mesh.Positions.Add(point);
mesh.TriangleIndices.Add(mesh.TriangleIndices.Count);
mesh.Normals.Add(normal);
}
}
public ExplodingMesh(MeshGeometry3D inputMesh, Point3D hitpos)
{
var mesh = MeshGeometryHelper.NoSharedVertices(inputMesh);
double cx, cy, cz;
cx = cy = cz = 0;
for (int i = 0; i < mesh.Positions.Count; i++)
{
cx += mesh.Positions[i].X;
cy += mesh.Positions[i].Y;
cz += mesh.Positions[i].Z;
}
int n = mesh.Positions.Count;
var center = new Point3D(cx / n, cy / n, cz / n);
integrator = new VerletIntegrator();
integrator.Resize(mesh.Positions.Count);
var r = new Random();
for (int i = 0; i < mesh.Positions.Count; i++)
{
var delta = mesh.Positions[i] - center;
delta.Normalize();
integrator.Positions[i] = mesh.Positions[i] + delta * (1 + r.NextDouble() * 2);
integrator.Positions0[i] = mesh.Positions[i];
integrator.Accelerations[i] = new Vector3D(0, 0, -1000);
integrator.InverseMass[i] = 0.01;
}
integrator.CreateConstraintsByMesh(mesh, 0.7);
integrator.AddFloor(0.3);
this.Mesh = mesh;
watch.Start();
}
public static bool ModelRayIntersection(WW3DModel model, Point3D rayOrig, Vector3D rayDir, out Point3D hitPos, out Vector3D hitSurfaceNormal, out double rayLength)
{
rayLength = double.MaxValue;
hitPos = new Point3D();
hitSurfaceNormal = new Vector3D();
var points = model.TriangleList();
var indices = model.IndexList();
for (int i = 0; i < indices.Length/3; ++i) {
Point3D pos;
Vector3D surfaceNormal;
double distance;
if (!TriangleRayIntersect(points[indices[i * 3 + 0]], points[indices[i * 3 + 1]], points[indices[i * 3 + 2]], rayOrig, rayDir, out pos, out surfaceNormal, out distance)) {
continue;
}
if (distance < rayLength) {
hitPos = pos;
hitSurfaceNormal = surfaceNormal;
rayLength = distance;
}
}
return rayLength != double.MaxValue;
}
public JointStatus()
{
position = new Point3D(0, 0, 0);
speed = new Point3D(0, 0, 0);
abs_speed = 0;
angle = 0;
}
private static bool InRange(Point3D v)
{
if (v.X < -1.0 || 1.0 < v.X || v.Y < -1.0 || 1.0 < v.Y) {
return false;
}
return true;
}
public KinectV1Settings(string uniqueID, int kinectNumber)
{
uniqueKinectID = uniqueID;
kinectID = kinectNumber;
//Set everything to the default value
colorImageMode = ColorImageFormat.RgbResolution640x480Fps30;
lineFrequency = PowerLineFrequency.SixtyHertz;
autoWhiteBalance = true;
autoExposure = true;
backlightMode = BacklightCompensationMode.AverageBrightness;
depthImageMode = DepthImageFormat.Resolution320x240Fps30;
isNearMode = false;
irON = true;
scaleDepthToReliableRange = false;
colorizeDepth = false;
mergeSkeletons = false;
kinectPosition = new Point3D(0, 0, 0);
kinectYaw = 0.0;
sendAcceleration = false;
sendAudioAngle = false;
audioTrackMode = AudioTrackingMode.Loudest;
sendColorImage = false;
sendDepthImage = false;
//Setup the options for the raw skeleton, irrespective of use
rawSkeletonSettings = new SkeletonSettings();
}
public WWLineSegment(Point3D startPos, Vector3D dirNormalized, double length, double intensity)
{
StartPos = startPos;
Direction = dirNormalized;
Length = length;
Intensity = intensity;
}
private ModelVisual3D makeTriangle(double x1, double y1, double z1, double x2, double y2, double z2, double x3, double y3, double z3)
{
MeshGeometry3D triangleMesh = new MeshGeometry3D();
Point3D point0 = new Point3D(x1, y1, z1);
Point3D point1 = new Point3D(x2, y2, z2);
Point3D point2 = new Point3D(x3, y3, z3);
triangleMesh.Positions.Add(point0);
triangleMesh.Positions.Add(point1);
triangleMesh.Positions.Add(point2);
triangleMesh.TriangleIndices.Add(0); // Важно - порядок обхода
triangleMesh.TriangleIndices.Add(2);
triangleMesh.TriangleIndices.Add(1);
Vector3D normal = new Vector3D(0, 1, 0);
triangleMesh.Normals.Add(normal);
triangleMesh.Normals.Add(normal);
triangleMesh.Normals.Add(normal);
Material material = new DiffuseMaterial(new SolidColorBrush(Colors.Red));
return new ModelVisual3D { Content = new GeometryModel3D(triangleMesh, material) };
}
public void CalculateJointForces(Point3D[, ,] framePoints)
{
int dim = 2;
jointForces = new Vector3D[dim, dim, dim];
for (int i = 0; i < dim; i++)
{
for (int j = 0; j < dim; j++)
{
for (int k = 0; k < dim; k++)
{
Vector3D P1;//kostka
Vector3D P2;//ramka
P1 = X[3 * i, 3 * j, 3 * k];
Point3D p = framePoints[i, j, k];
P2 = new Vector3D(p.X, p.Y, p.Z);
if (IsDampingActive)
CollisionChecker.TrimPoint(ref P2);
Vector3D V1 = V0[3 * i, 3 * j, 3 * k];
Vector3D I0 = new Vector3D(0, 0, 0); //długość spoczynkowa sprężynki połączonej z ramką ma być 0
Vector3D diff = P1 - P2;
if (Math.Sqrt(diff.X * diff.X + diff.Y * diff.Y + diff.Z * diff.Z) < 0.00001)
jointForces[i, j, k] = new Vector3D(0, 0, 0);
else
jointForces[i, j, k] = springFrame.GetCurrentForce(P1, P2, I0, V1);
}
}
}
}
private System.Windows.Media.Media3D.Point3D FilteredMatrixToPoint(EigenWrapper.Matrix matrix)
{
System.Windows.Media.Media3D.Point3D point = new System.Windows.Media.Media3D.Point3D();
point.X = matrix[0, 0];
point.Y = matrix[3, 0];
point.Z = matrix[6, 0];
return(point);
}
public void Plot(Shapes.Point point, p3D.Point3D center, double rot_x, double rot_y, double rot_z)
{
string series = pointSeriesName(point);
chart.Series[series].Points.Clear();
pointPlotter(point, rot_x, rot_y, rot_z, center);
chartSizer(chart);
}
private void _tracker_GazeDataReceived(object sender, GazeDataEventArgs e)
{
// Convert to centimeters
const double D = 10.0;
_leftPos.X = e.GazeDataItem.LeftEyePosition3D.X / D;
_leftPos.Y = e.GazeDataItem.LeftEyePosition3D.Y / D;
_leftPos.Z = e.GazeDataItem.LeftEyePosition3D.Z / D;
_rightPos.X = e.GazeDataItem.RightEyePosition3D.X / D;
_rightPos.Y = e.GazeDataItem.RightEyePosition3D.Y / D;
_rightPos.Z = e.GazeDataItem.RightEyePosition3D.Z / D;
_leftGaze.X = e.GazeDataItem.LeftGazePoint3D.X / D;
_leftGaze.Y = e.GazeDataItem.LeftGazePoint3D.Y / D;
_leftGaze.Z = e.GazeDataItem.LeftGazePoint3D.Z / D;
_rightGaze.X = e.GazeDataItem.RightGazePoint3D.X / D;
_rightGaze.Y = e.GazeDataItem.RightGazePoint3D.Y / D;
_rightGaze.Z = e.GazeDataItem.RightGazePoint3D.Z / D;
// Set which eyes to show
_showLeft = e.GazeDataItem.LeftValidity < 2;
_showRight = e.GazeDataItem.RightValidity < 2;
Action update = delegate()
{
if (_showLeft)
{
_eyePair.LeftEyePosition = _leftPos;
_leftGazeVector.Point1 = _leftPos;
_leftGazeVector.Point2 = _leftGaze;
}
else
{
Point3D farAway = new Point3D(1000.0, 1000.0, 1000.0);
_eyePair.LeftEyePosition = farAway;
_leftGazeVector.Point1 = farAway;
_leftGazeVector.Point2 = farAway;
}
if (_showRight)
{
_eyePair.RightEyePosition = _rightPos;
_rightGazeVector.Point1 = _rightPos;
_rightGazeVector.Point2 = _rightGaze;
}
else
{
Point3D farAway = new Point3D(1000.0, 1000.0, 1000.0);
_eyePair.RightEyePosition = farAway;
_rightGazeVector.Point1 = farAway;
_rightGazeVector.Point2 = farAway;
}
};
Dispatcher.BeginInvoke(update);
}
private void SafeAddPoint(MeshGeometry3D mesh, Point3D point)
{
if (++_vertexCount >= int.MaxValue)
{
return;
}
mesh.Positions.Add(point);
}
private EigenWrapper.Matrix PointToObMatrix(System.Windows.Media.Media3D.Point3D point)
{
EigenWrapper.Matrix matrix = new EigenWrapper.Matrix(3, 1);
matrix[0, 0] = point.X;
matrix[1, 0] = point.Y;
matrix[2, 0] = point.Z;
return(matrix);
}
public void Plot(TextAnnotation label, string text, p3D.Point3D center, double rot_x, double rot_y, double rot_z)
{
label.Text = text;
Convert3DTo2D(rot_x, rot_y, rot_z, center.X, center.Y, center.Z, ref x, ref y, ref z);
chart.Annotations[label.Name].AxisX = chart.ChartAreas[0].AxisX;
chart.Annotations[label.Name].AxisY = chart.ChartAreas[0].AxisY;
chart.Annotations[label.Name].AnchorX = x;
chart.Annotations[label.Name].AnchorY = y;
}
/// <summary>
/// Creates a new instance of Plane3D, that is given by 3 points.
/// </summary>
/// <param name="p1">First point laying in the plane. This one is taken as origin point.</param>
/// <param name="p2">Second point laying in the plane.</param>
/// <param name="p3">Third point laying in the plane.</param>
public Plane3D(WM.Point3D p1, WM.Point3D p2, WM.Point3D p3)
{
point = p1;
var v1 = p2 - p1;
var v2 = p3 - p1;
normal = WM.Vector3D.CrossProduct(v1, v2);
normal.Normalize();
}
private void UpdateDepthImagePointCloud()
{
if (this.depthImage == null || this.depthImage.Resource == null || this.intrinsics == null || this.position == null)
{
this.PointCloud.SetCurrentValue(this.SynthesizeMessage <List <Windows.Point3D> >(null));
return;
}
int width = this.depthImage.Resource.Width;
int height = this.depthImage.Resource.Height;
var pointsArray = new Windows.Point3D[width, height];
var points = new List <Windows.Point3D>();
double scale = this.depthImage.Resource.DepthValueToMetersScaleFactor;
unsafe
{
ushort *depthFrame = (ushort *)((byte *)this.depthImage.Resource.ImageData.ToPointer());
Parallel.For(0, height, iy =>
{
var row = iy * width;
var point = Vector <double> .Build.Dense(4);
point[3] = 1;
for (int ix = 0; ix < width; ix += this.sparsity)
{
int i = row + ix;
var d = depthFrame[i];
if (d != 0)
{
var dscaled = d * scale;
var cameraSpacePoint = this.cameraSpaceMapping[ix, iy];
point[0] = dscaled * cameraSpacePoint.X;
point[1] = dscaled * cameraSpacePoint.Y;
point[2] = dscaled * cameraSpacePoint.Z;
var transformed = this.position.Multiply(point);
pointsArray[ix, iy] = new Windows.Point3D(transformed[0], transformed[1], transformed[2]);
}
}
});
for (int iy = 0; iy < height; iy += this.sparsity)
{
var row = iy * width;
for (int ix = 0; ix < width; ix += this.sparsity)
{
int i = row + ix;
if (depthFrame[i] != 0)
{
points.Add(pointsArray[ix, iy]);
}
}
}
}
this.PointCloud.SetCurrentValue(this.SynthesizeMessage(points));
}
public void SendToolMoveMessage(int toolId, Point3D position, Vector3D direction, double length, double radius)
{
Messenger.Default.Send(new ToolMoveMessage()
{
Position = position,
Direction = direction,
Length = length,
Radius = radius
});
}
internal static WM.Point3D Multiply(WM.Point3D point, Matrix44 matrix)
{
WM.Point3D newPoint = new WM.Point3D
{
X = (point.X * matrix.matrixElement[0, 0]) + (point.Y * matrix.matrixElement[1, 0]) + (point.Z * matrix.matrixElement[2, 0]),
Y = (point.X * matrix.matrixElement[0, 1]) + (point.Y * matrix.matrixElement[1, 1]) + (point.Z * matrix.matrixElement[2, 1]),
Z = (point.X * matrix.matrixElement[0, 2]) + (point.Y * matrix.matrixElement[1, 2]) + (point.Z * matrix.matrixElement[2, 2])
};
return(newPoint);
}
public RotateEventHandler(MVMIH.IRotationHandle rotationHandle)
{
_rotationHandle = rotationHandle;
_matrix = GetTranslateTransformation(rotationHandle);
_eleRotMatrix = Converters.StaticTransformationConverter.Convert((rotationHandle as IMachineElement).Parent.Parent.Transformation);
_rotationDirection = _eleRotMatrix.Transform(GetRotationDirection(rotationHandle));
_rotationCenter = _eleRotMatrix.Transform(GetRotationCenter((rotationHandle as IMachineElement).Parent.Parent));
_rotationHandle.StartRotate();
}
private void UpdateBillboard()
{
if (this.CurrentData != null)
{
var origin = this.CurrentData.Item2;
var pos = new Win3D.Point3D(origin.X, origin.Y, origin.Z + (this.BillboardHeightCm / 100.0));
var text = this.CurrentData.Item1.ToString();
this.Billboard.SetCurrentValue(this.SynthesizeMessage(Tuple.Create(pos, text)));
}
}
/// <summary>
/// RANSAC, detects and updates groundPlane
/// </summary>
private static void UpdateGround()
{
int maxIterations = 100;
double threshDist = 0.05;
int iterations = 0;
int bestCount = 0;
List <int> RansacPointsIndexes = new List <int>();
Plane bestPlane = new Plane();
Random rnd = new Random();
foreach (var point in allPoints)
{
if (point.Value.Y < 0 + robotHeight)
{
RansacPointsIndexes.Add(point.Key);
}
}
while (iterations < maxIterations)
{
Media3D.Point3D p = allPoints[RansacPointsIndexes.ElementAt(rnd.Next(0, RansacPointsIndexes.Count()))];
Vector3 point1 = new Vector3((float)p.X, (float)p.Y, (float)p.Z);
p = allPoints[RansacPointsIndexes.ElementAt(rnd.Next(0, RansacPointsIndexes.Count()))];
Vector3 point2 = new Vector3((float)p.X, (float)p.Y, (float)p.Z);
p = allPoints[RansacPointsIndexes.ElementAt(rnd.Next(0, RansacPointsIndexes.Count()))];
Vector3 point3 = new Vector3((float)p.X, (float)p.Y, (float)p.Z);
Plane plane = new Plane(point1, point2, point3);
int count = 0;
foreach (var index in RansacPointsIndexes)
{
if (Math.Abs(ComputeDistance(allPoints[index], plane)) < threshDist)
{
count++;
}
}
if (count > bestCount)
{
bestPlane = plane;
bestCount = count;
}
iterations++;
}
groundPlane = bestPlane;
}
public void UpdateBoxes(double[] heights)
{
for (int i = 0; i < Boxes.Count; i++)
{
BoxVisual3D box = Boxes[i];
Point3D center = box.Center;
center.Z = heights[i] / 2;
box.Center = center;
box.Height = heights[i];
}
}
// Add a triangle to the indicated mesh.
private void AddTriangle(MeshGeometry3D mesh,
Point3D point1, Point3D point2, Point3D point3)
{
// Get the points' indices.
var index1 = AddPoint(mesh.Positions, point1);
var index2 = AddPoint(mesh.Positions, point2);
var index3 = AddPoint(mesh.Positions, point3);
// Create the triangle.
mesh.TriangleIndices.Add(index1);
mesh.TriangleIndices.Add(index2);
mesh.TriangleIndices.Add(index3);
}
private void UpdateScreen()
{
var config = _tracker.GetXConfiguration();
Point3D ul = ToWpfPoint3D(config.UpperLeft, 0.1);
Point3D ur = ToWpfPoint3D(config.UpperRight, 0.1);
Point3D ll = ToWpfPoint3D(config.LowerLeft, 0.1);
Screen screen = new Screen(ul, ur, ll);
visualModel.Children.Remove(_screen);
_screen = screen;
visualModel.Children.Add(_screen);
}
